// #include "ros/ros.h"
// #include <iostream>
// #include <string>
// #include <thread>
// #include <unistd.h>
// #include <time.h>
// #include <deque>
// #include "r2control/main_control.h"
// #include "r2control/dynamic_tf_handle.h"

// // #define offset -0.052f
// #define offset -0.052f
// #define capcity 3

// using namespace std;

// //实例化全局串口发送对象
// R2serHandler local_ser;
// DyncTFhandle dync_handle_tf;
// //初始化串口结构体
// ser_setting ser0{"/dev/ttyACM0",115200,200};
// ser_setting ser1{"/dev/ttyACM1",115200,200};
// //初始化一个用于滤波的双向队列
// deque <Postion>  _odom_tf_deque;


// void velCallBcak(const geometry_msgs::Twist& msg)
// {
//     unsigned char *goal_buf;
//     usleep(10510);
//     //由于/cmd_vel订阅的消息类型是Twist是Euler的形式
//     ROS_INFO("the cmd_vel(v_x,v_y,omega) is %f,%f,%f",msg.linear.x,msg.linear.y,(msg.angular.z));
//     goal_buf=local_ser.writeSpeedGoal(msg.linear.x,msg.linear.y,msg.angular.z);
//     // for (int i=0;i<21;i++)
//     // ROS_INFO("%d",goal_buf[i]);
//     // writeData(goal_buf,0x01);
// }


// void odomCallBcak(const nav_msgs::Odometry::ConstPtr &msg)
// {
//     unsigned char *speed_now_buf,*goal_now_buf;
//     usleep(10510);
//     Euler *PRY_data=new Euler;//由于yaw本身已经旋转了-45度
//     DyncTFhandle::transformData *body_to_world=new DyncTFhandle::transformData{"aft_mapped","map",Eigen::Vector3d(0,-offset,0),Eigen::Vector3d(0,0,0),Euler{0,0,-0.7853981}};     //给carbody_to_world结构体赋值,Eigen::Vector3d(0,0,0),Eigen::Vector3d(0,0,0),Euler{0,0,-0.785398
//     DyncTFhandle::transformData *car_body_to_map=new DyncTFhandle::transformData{"car_body","map"};     //由于车体实则是以雷达body作为旋转的圆心，因此需要再构建一层坐标映射关系
//     //动态的速度关系
//     // ROS_INFO("the odom(v_x,v_y,omega) is %f,%f,%f",msg->twist.twist.linear.x,msg->twist.twist.linear.y,msg->twist.twist.angular.z);
//     // speed_now_buf=local_ser.writeSpeedNow(msg->twist.twist.linear.x,msg->twist.twist.linear.y,msg->twist.twist.angular.z);
//     //静态坐标系转换，雷达坐标与地图坐标的相互关系是不变的
//     dync_handle_tf.tf_quaternion2RPY(msg->pose.pose.orientation,PRY_data);
     
//     Eigen::Vector3d p1(msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z);
//     Eigen::Vector3d p2=dync_handle_tf.force_transform_pose(p1,body_to_world);//转换到地图坐标系

//     //动态坐标系转换，车体坐标与地图坐标的相互关系是变化的，取决于雷达里程计的旋转
//     car_body_to_map->pose_RPY_Parent.yaw=PRY_data->yaw+0.7853981;//相对map来说，车体坐标系，是转换后的雷达坐标正向旋转45度
//     car_body_to_map->pose_XYZ_Child=Eigen::Vector3d(0,0,0);//车体坐标点，是先在雷达坐标系下，所以没有位移关系
//     car_body_to_map->pose_XYZ_Parent=p2;//相对于地图坐标系，位移矩阵即是雷达坐标点的本身
//     Eigen::Vector3d point_car_origin=Eigen::Vector3d(0,-offset,0);
//     Eigen::Vector3d p3=dync_handle_tf.force_transform_pose(point_car_origin,car_body_to_map);
//     Postion *now_pos=new Postion{p3[0],p3[1],PRY_data->yaw}; //创建一个位置结构体
//     //向对列内添加转换后的里程计数据
//     // _odom_tf_deque.push_back(*now_pos);
//     ROS_INFO("x=%f",now_pos->x);
//     ROS_INFO("y=%f",now_pos->y);
//     ROS_INFO("yaw=%f",PRY_data->yaw);

//     goal_now_buf=local_ser.writePosNow(now_pos);

//     // ROS_INFO("x2=%.3f,y2=%.3f,z2=%.3f",p1[0],p1[1],PRY_data->yaw*57.29); 
//     // ROS_INFO("x3=%.3f,y3=%.3f,z3=%.3f",now_pos->x,now_pos->y,(now_pos->yaw+0.7853981)*57.29);
//     // if (_odom_tf_deque.size()>capcity)    //当队列容量超过上限时清掉第一个
//     // _odom_tf_deque.pop_front();
//     //清空内存
//     // for (int i=0;i<24;i++)
//     // ROS_INFO("%d",goal_now_buf[i]);
//     // writeData(speed_now_buf,0x01);
//     try
//      {
//         writeData(goal_now_buf,0x01);
//      }
//      catch(const std::exception& e)
//      {
//         ROS_ERROR("No exist data");
//         try{
//             // cout<<Ros_to_Stm32.getPort()<<endl;
//             if (Ros_to_Stm32.getPort()=="/dev/ttyACM1"){
//                 Ros_to_Stm32.setPort("/dev/ttyACM0");
//                 if(!Ros_to_Stm32.isOpen())
//                 Ros_to_Stm32.open();
//             }else{
//                 Ros_to_Stm32.setPort("/dev/ttyACM1");
//                 if(!Ros_to_Stm32.isOpen())
//                 Ros_to_Stm32.open();
//             }
//             // serialInit(&ser1);//串口初始化
//         }
//         catch(const std::exception& e){
//             // serialInit(&ser0);//串口初始化
//             ROS_ERROR("No exist data everyone");
//         }
//     }
//     delete PRY_data,now_pos,body_to_world,car_body_to_map;
// }

// void data_solve()
// {
//     //初始化全部的sum计算平均值
//     static unsigned char *goal_now_buf;
//     DyncTFhandle::transformData *car_body_avg_to_map=new DyncTFhandle::transformData{"car_avg_body","map"};     //由于车体实则是以雷达body作为旋转的圆心，因此需要再构建一层坐标映射关系
//     while (1)
//     {
//         usleep(10510);
//         // time_t start=time(NULL);

//         if(_odom_tf_deque.size()>0){
//             double sum_x=0,  sum_y=0,  sum_yaw=0;
//             for (deque<Postion>::const_iterator it =_odom_tf_deque.begin(); it!=_odom_tf_deque.end();it++)
//                 {
//                     sum_x+=it->x;
//                     sum_y+=it->y;
//                     sum_yaw+=it->yaw;
//                 }
//             //创建接收位置信息的结构体
//             Postion *pos_out=new Postion{sum_x/_odom_tf_deque.size(),sum_y/_odom_tf_deque.size(),sum_yaw/_odom_tf_deque.size()};
//             // ROS_INFO("y=%f",pos_out->y);
//             // ROS_INFO("x=%f",pos_out->x);

//             // car_body_avg_to_map->pose_XYZ_Parent=Eigen::Vector3d(0,0,0);//车体坐标点，是先在雷达坐标系下，所以没有位移关系
//             // Eigen::Vector3d pos_now=Eigen::Vector3d(pos_out->x,pos_out->y,0);
//             // car_body_avg_to_map->pose_XYZ_Child=pos_now;//相对于地图坐标系，位移矩阵即是雷达坐标点的本身
//             // car_body_avg_to_map->pose_RPY_Child.yaw=pos_out->yaw;//相对map来说，车体坐标系，是转换后的雷达坐标正向旋转45度
//             // dync_handle_tf.dync_pub(car_body_avg_to_map);//发布动态的坐标，这是转换后的均值坐标

//             //发送数据转换
//             goal_now_buf=local_ser.writePosNow(pos_out);
//             ROS_INFO("yaw2=%f",pos_out->y);

//             // for (int i=0;i<24;i++)
//             // ROS_INFO("%d",goal_now_buf[i]);
//             // writeData(goal_now_buf,0x01);
//             delete pos_out;//清空内存
//         }
//         else
//         {
//             // ROS_INFO("size=%d",(int)_odom_tf_deque.size());
//         }
//     }
// }

// int main(int argc,char **argv)
// {
//     try
//      {
//          serialInit(&ser0);//串口初始化
//      }
//      catch(const std::exception& e)
//      {
//          ROS_ERROR("UNSUCCESSFULLY START");
//         //  return false;
//     }
//     //激活ROS
//     setlocale(LC_ALL,"");
//     ros::init(argc, argv, "FUCKING");
//     ros::NodeHandle nh; 
//     ros::Rate loop_rate(1);//定义定时器循环时间长度
//     //发布雷达odom与车体odom的tf关系
//     DyncTFhandle::transformData *car_to_cam=new DyncTFhandle::transformData{"car_body","aft_mapped",Eigen::Vector3d(0,-offset,0),Eigen::Vector3d(0,0,0),Euler{0,0,0}}; 
//     thread receive_thread(&DyncTFhandle::static_pub,&dync_handle_tf,(car_to_cam));
//     receive_thread.detach();
   
//     // thread data_solve_thread(data_solve);
//     // data_solve_thread.detach();

//     // ros::Subscriber sub_vel=nh.subscribe("/cmd_vel",100,velCallBcak);
//     ros::Subscriber sub_odom=nh.subscribe("/aft_mapped_to_init",100,odomCallBcak);
//     ros::AsyncSpinner spinner(4);
//     spinner.start();
//     ros::waitForShutdown();
    
//     return 0;
// }
#include "ros/ros.h"
#include <iostream>
#include <string>
#include <thread>
#include <unistd.h>
#include <time.h>
#include <deque>
#include "r2control/main_control.h"
#include "r2control/dynamic_tf_handle.h"

// #define offset -0.052f
#define offset -0.052f
#define capcity 3

using namespace std;

//实例化全局串口发送对象
R2serHandler local_ser;
DyncTFhandle dync_handle_tf;
//初始化串口结构体
ser_setting ser0{"/dev/ttyACM0",115200,200};
ser_setting ser1{"/dev/ttyACM1",115200,200};
//初始化一个用于滤波的双向队列
deque <Postion>  _odom_tf_deque;


void velCallBcak(const geometry_msgs::Twist& msg)
{
    unsigned char *goal_buf;
    usleep(10510);
    //由于/cmd_vel订阅的消息类型是Twist是Euler的形式
    ROS_INFO("the cmd_vel(v_x,v_y,omega) is %f,%f,%f",msg.linear.x,msg.linear.y,(msg.angular.z));
    goal_buf=local_ser.writeSpeedGoal(msg.linear.x,msg.linear.y,msg.angular.z);
    // for (int i=0;i<21;i++)
    // ROS_INFO("%d",goal_buf[i]);
    // writeData(goal_buf,0x01);
}


void odomCallBcak(const nav_msgs::Odometry::ConstPtr &msg)
{
    unsigned char *speed_now_buf,*goal_now_buf;
    usleep(10510);
    Euler *PRY_data=new Euler;//由于yaw本身已经旋转了-45度
    DyncTFhandle::transformData *body_to_world=new DyncTFhandle::transformData{"body","map",Eigen::Vector3d(0,-offset,0),Eigen::Vector3d(0,0,0),Euler{0,0,-0.7853981}};     //给carbody_to_world结构体赋值,Eigen::Vector3d(0,0,0),Eigen::Vector3d(0,0,0),Euler{0,0,-0.785398
    DyncTFhandle::transformData *car_body_to_map=new DyncTFhandle::transformData{"car_body","map"};     //由于车体实则是以雷达body作为旋转的圆心，因此需要再构建一层坐标映射关系
    //动态的速度关系
    // ROS_INFO("the odom(v_x,v_y,omega) is %f,%f,%f",msg->twist.twist.linear.x,msg->twist.twist.linear.y,msg->twist.twist.angular.z);
    // speed_now_buf=local_ser.writeSpeedNow(msg->twist.twist.linear.x,msg->twist.twist.linear.y,msg->twist.twist.angular.z);
    //静态坐标系转换，雷达坐标与地图坐标的相互关系是不变的
    dync_handle_tf.tf_quaternion2RPY(msg->pose.pose.orientation,PRY_data);
     
    Eigen::Vector3d p1(msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z);
    Eigen::Vector3d p2=dync_handle_tf.force_transform_pose(p1,body_to_world);//转换到地图坐标系

    //动态坐标系转换，车体坐标与地图坐标的相互关系是变化的，取决于雷达里程计的旋转
    car_body_to_map->pose_RPY_Parent.yaw=PRY_data->yaw+0.7853981;//相对map来说，车体坐标系，是转换后的雷达坐标正向旋转45度
    car_body_to_map->pose_XYZ_Child=Eigen::Vector3d(0,0,0);//车体坐标点，是先在雷达坐标系下，所以没有位移关系
    car_body_to_map->pose_XYZ_Parent=p2;//相对于地图坐标系，位移矩阵即是雷达坐标点的本身
    Eigen::Vector3d point_car_origin=Eigen::Vector3d(0,-offset,0);
    Eigen::Vector3d p3=dync_handle_tf.force_transform_pose(point_car_origin,car_body_to_map);
    Postion *now_pos=new Postion{p3[0],p3[1],PRY_data->yaw}; //创建一个位置结构体
    //向对列内添加转换后的里程计数据
    // _odom_tf_deque.push_back(*now_pos);
    ROS_INFO("x=%f",now_pos->x);
    ROS_INFO("y=%f",now_pos->y);
    ROS_INFO("yaw=%f",PRY_data->yaw);

    goal_now_buf=local_ser.writePosNow(now_pos);

    // ROS_INFO("x2=%.3f,y2=%.3f,z2=%.3f",p1[0],p1[1],PRY_data->yaw*57.29); 
    // ROS_INFO("x3=%.3f,y3=%.3f,z3=%.3f",now_pos->x,now_pos->y,(now_pos->yaw+0.7853981)*57.29);
    // if (_odom_tf_deque.size()>capcity)    //当队列容量超过上限时清掉第一个
    // _odom_tf_deque.pop_front();
    //清空内存
    // for (int i=0;i<24;i++)
    // ROS_INFO("%d",goal_now_buf[i]);
    // writeData(speed_now_buf,0x01);
    try
     {
        writeData(goal_now_buf,0x01);
     }
     catch(const std::exception& e)
     {
        ROS_ERROR("No exist data");
        try{
            // cout<<Ros_to_Stm32.getPort()<<endl;
            if (Ros_to_Stm32.getPort()=="/dev/ttyACM1"){
                Ros_to_Stm32.setPort("/dev/ttyACM0");
                if(!Ros_to_Stm32.isOpen())
                Ros_to_Stm32.open();
            }else{
                Ros_to_Stm32.setPort("/dev/ttyACM1");
                if(!Ros_to_Stm32.isOpen())
                Ros_to_Stm32.open();
            }
            // serialInit(&ser1);//串口初始化
        }
        catch(const std::exception& e){
            // serialInit(&ser0);//串口初始化
            ROS_ERROR("No exist data everyone");
        }
    }
    delete PRY_data,now_pos,body_to_world,car_body_to_map;
}

void data_solve()
{
    //初始化全部的sum计算平均值
    static unsigned char *goal_now_buf;
    DyncTFhandle::transformData *car_body_avg_to_map=new DyncTFhandle::transformData{"car_avg_body","map"};     //由于车体实则是以雷达body作为旋转的圆心，因此需要再构建一层坐标映射关系
    while (1)
    {
        usleep(10510);
        // time_t start=time(NULL);

        if(_odom_tf_deque.size()>0){
            double sum_x=0,  sum_y=0,  sum_yaw=0;
            for (deque<Postion>::const_iterator it =_odom_tf_deque.begin(); it!=_odom_tf_deque.end();it++)
                {
                    sum_x+=it->x;
                    sum_y+=it->y;
                    sum_yaw+=it->yaw;
                }
            //创建接收位置信息的结构体
            Postion *pos_out=new Postion{sum_x/_odom_tf_deque.size(),sum_y/_odom_tf_deque.size(),sum_yaw/_odom_tf_deque.size()};
            // ROS_INFO("y=%f",pos_out->y);
            // ROS_INFO("x=%f",pos_out->x);

            // car_body_avg_to_map->pose_XYZ_Parent=Eigen::Vector3d(0,0,0);//车体坐标点，是先在雷达坐标系下，所以没有位移关系
            // Eigen::Vector3d pos_now=Eigen::Vector3d(pos_out->x,pos_out->y,0);
            // car_body_avg_to_map->pose_XYZ_Child=pos_now;//相对于地图坐标系，位移矩阵即是雷达坐标点的本身
            // car_body_avg_to_map->pose_RPY_Child.yaw=pos_out->yaw;//相对map来说，车体坐标系，是转换后的雷达坐标正向旋转45度
            // dync_handle_tf.dync_pub(car_body_avg_to_map);//发布动态的坐标，这是转换后的均值坐标

            //发送数据转换
            goal_now_buf=local_ser.writePosNow(pos_out);
            ROS_INFO("yaw2=%f",pos_out->y);

            // for (int i=0;i<24;i++)
            // ROS_INFO("%d",goal_now_buf[i]);
            // writeData(goal_now_buf,0x01);
            delete pos_out;//清空内存
        }
        else
        {
            // ROS_INFO("size=%d",(int)_odom_tf_deque.size());
        }
    }
}

int main(int argc,char **argv)
{
    try
     {
         serialInit(&ser0);//串口初始化
     }
     catch(const std::exception& e)
     {
         ROS_ERROR("UNSUCCESSFULLY START");
        //  return false;
    }
    //激活ROS
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "FUCKING");
    ros::NodeHandle nh; 
    ros::Rate loop_rate(1);//定义定时器循环时间长度
    //发布雷达odom与车体odom的tf关系
    DyncTFhandle::transformData *car_to_cam=new DyncTFhandle::transformData{"car_body","body",Eigen::Vector3d(0,-offset,0),Eigen::Vector3d(0,0,0),Euler{0,0,0}}; 
    thread receive_thread(&DyncTFhandle::static_pub,&dync_handle_tf,(car_to_cam));
    receive_thread.detach();
   
    // thread data_solve_thread(data_solve);
    // data_solve_thread.detach();

    // ros::Subscriber sub_vel=nh.subscribe("/cmd_vel",100,velCallBcak);
    ros::Subscriber sub_odom=nh.subscribe("/Odometry",100,odomCallBcak);
    ros::AsyncSpinner spinner(4);
    spinner.start();
    ros::waitForShutdown();
    
    return 0;
}